%% 目标函数定义
function fitness = objective_function(params)
    % 假设参数为 [x1, x2]
    x1 = params(1);
    x2 = params(2);
    
    assignin("base", 'x1', x1);
    assignin("base", 'x2', x2);

    % 执行模型仿真
    filename = 'qiyueqi_R2023a.mdl';
    res = sim(filename);
    o1 = res.ScopeData.signals(1).values;
    o2 = res.ScopeData.signals(2).values;

    % 计算H1和H2
    H1 = max(o1) - min(o1);
    H2 = max(o2) - min(o2);

    % 如果H1, H2都小于阈值，则进行惩罚，返回更高的适应度
    if H1 < 1.2 && H2 < 1.2
        fitness = H1 + H2;  % 总幅度
    else
        fitness = 9999;  % 惩罚值，如果不满足条件
    end
end

%% 粒子群优化
nvars = 2;  % 优化的变量个数
lb = [-1, -1];  % 参数下界
ub = [1, 1];  % 参数上界

% 粒子群优化的参数
%%options = optimoptions('particleswarm', 'SwarmSize', 5, 'MaxIterations', 5, 'Display', 'iter');

% 执行粒子群优化
%%[bestParams, bestFitness] = particleswarm(@objective_function, nvars, lb, ub, options);

% 显示最优结果
%%fprintf('最优参数：x1 = %.10f, x2 = %.10f\n', bestParams(1), bestParams(2));
%%fprintf('最优适应度值：%.10f\n', bestFitness);

%% 创建x1, x2网格并计算对应的fitness值
x1_range = linspace(-1, 1, 20);  % x1的取值范围
x2_range = linspace(-1, 1, 20);  % x2的取值范围

% 创建网格
[X1, X2] = meshgrid(x1_range, x2_range);

% 计算对应的fitness值
FitnessValues = NaN(size(X1));  % 初始化fitness矩阵
for i = 1:size(X1, 1)
    for j = 1:size(X1, 2)
        params = [X1(i, j), X2(i, j)];
        fitness = objective_function(params);
        if fitness < 9999
            FitnessValues(i, j) = fitness;  % 只存储fitness小于9999的值
        end
    end
end

%% 绘制三维柱状图
figure;
surf(X1, X2, FitnessValues, 'EdgeColor', 'none');
colorbar;  % 显示颜色条
caxis([1.2 3]);  % 设置颜色条的范围
xlabel('x1');
ylabel('x2');
zlabel('Fitness');
title('Fitness Function Surface');
colormap jet;  % 颜色渐变
